% v10: pick "corner-like" 4 random points
% v12: pick 4 "correct" corners
% v14: "correctness" including area threshold
% v16: unify coordinates (math coord. and image coord. of model)
% v20: fix homography calculation BUG (reprojection)
% v21: inner point check
% v25: user input
% v30: initialization & consecutive steps
% v40: use 4 points homography
% v50: change model data structure to a set of segments
% v52: improve preprocessing -> less points in point cloud, less corners
% v55: remove "proper" & "small movement" check & use edge neighbor
% Client Layer
function HomoRANSACtester_v57
clc;clear;close all; 


% Loading images
% from camera
% dir = 'D:\WORK\WORK\NTUST\projects\DIRECTIONS\OpenCV & Robot Vision - FIRST\tracking\clean pattern sequence\linear pattern - normal bk gnd';
% from wc
% dir = 'D:\WORK\WORK\NTUST\projects\DIRECTIONS\OpenCV & Robot Vision - FIRST\tracking\clean pattern sequence\linear pattern - normal bk gnd - wc';
% to test pose estimation
dir = 'D:\WORK\WORK\NTUST\projects\DIRECTIONS\OpenCV & Robot Vision - FIRST\pose estimation\Homography';

% for i = 1:10:200
fig = figure('name','model based homography RANSAC tracking','Position', [100 500 1100 400]);

for i = 1:5:413
    i
    tic
    
    f1 = [dir '\frame_' num2str(i,'%03d') '.jpg'];
    
    % fig = figure('name','model based homography RANSAC tracking');
    
    img = imread(f1);
    
    % Create model
    if i==1,      
        % Create a model
        % assumed measurements
%         d = 7.5;
%         c = 18;
%         a = 90;
%         b = 90;
        % real measurements (mm)
        d = 18; 
        c = 33;
        a = 180;
        b = 175;
%         res = 0.2;
        
%         model = CreatePatternModel([a b c d res]); % point cloud model
        model = CreatePatternModel([a b c d]); % point cloud model
        model = flipMDL(model,'Ox');
        model = shiftMDL(model,[0 b]);
        model_para = XtrackMDLpara(model);
        
        modeli = model;
        modeli_para = model_para;
    end
    
    % Data preparing      
    [pts r1 c1 imgH imgW] = PreProcessing(fig,img);

    % number of all choices of 4 corners:
%     cases = factorial(size(r1,1))/factorial(size(r1,1)-4)
%     cnt = 0;

    % Prepare to call RANSAC
    iterNum = 100;
    
    thDist = 3; %2; %1.5;
    thInlrRatio = 0.001;

    % call RANSAC
    if i==1, 
%         % Create a model
%         d = 7.5;
%         c = 18;
%         a = 90;
%         b = 90;
% %         res = 0.2;
%         
% %         model = CreatePatternModel([a b c d res]); % point cloud model
%         model = CreatePatternModel([a b c d]); % point cloud model
%         model = flipMDL(model,'Ox');
%         model = shiftMDL(model,[0 b]);
%         MdlPts(1,:) = [0 a a 0 d+c]; % math coord.
%         MdlPts(2,:) = [b b 0 0 b-d-c];
        
        [modeli, modeli_para] = RSpatternTrackerINIT(model,pts,r1,c1,iterNum,thDist,thInlrRatio);
    
    else
        
        [modeli, modeli_para] = RSpatternTrackerRUN(modeli,modeli_para,pts,r1,c1,iterNum,thDist,thInlrRatio);
    end
    
%     hold on
     t = toc;
    
%     if (iscell(modeli)), 
%         plotmodel(fig,modeli);

          % plot HERE
        plotmodel(fig,modeli,'image',imgH);
        
        xlabel(['Elabpsed Time =' num2str(t,'%0.3f')]);
        title('Model Based Homography RANSAC Tracking');
          % end plot
        
%     else
%         error('No Found Solution!!');
%     end;
    
    
%     hold off   
    
    input('Press Enter'); 
%     pause(0.3);

%     clf;  % clear figure
%     close;
       
    % falseCase = cnt/iterNum*100
    % q
    % runcase = iterNum-cnt
    
end

    function [points, r ,c, imgH, imgW] = PreProcessing(fig,img)
        
        figure(fig);
        hold on;
%         clf;
        % prepare data here: image processing
        
        % img=imread('pic01.JPG');
        % img=imread('frame_396.jpg');
        % img=imread('frame_001.jpg');
        % img=imread('frame_100.jpg');
        % img=imread('frame_220.jpg');
        
        img=rgb2gray(img); % used when input is RGB image
        hold on;
        subplot(1,2,1);
        imshow(img);
        xlabel('Original Image');
        
        % Scale down to suitable size
        
        img=imresize(img,1/2);
        
        % Edge detection: binary image BW
        BW = edge(img,'canny');
        subplot(1,2,2); % draw edge image in demonstration plot
        imshow(BW);
        
        % Extract corners
        thresh = 5;
        [cim1, r1, c1] = harris(img, 3, thresh, 3);
        
        % store original coord
        r1_0 = r1;
        c1_0 = c1;
        
        % Convert img2mathCoord
        r1 = -r1;
        r1 = r1 + size(BW,1);  % BUG: size(img) <> size(BW) :))
        % imshow(img), hold on, plot(c1,r1,'r+');
        
        % Extract points with [row,col,v] = find(X, ...)
        [row,col] = find(BW~=0);
        
        % store original coord
        row0 = row;
        col0 = col;
        
        % Convert img2mathCorod
        row = -row;
        row = row + size(BW,1);
        
        % Extract a subset of original data
        if size(row,1) > 150, %1000,
%             id = randIndex(size(row,1),1000);
            id = randIndex(size(row,1),150);
            pts(1,:)=col(id(:));  % take 50~70% only not all -> try to use uniform random selection
            pts(2,:)=row(id(:));
            pts0(1,:)=col0(id(:));
            pts0(2,:)=row0(id(:));
        end
              
%         figure,
%         hold; 

        % PLOT with math coord
%         axis('equal')
%         subplot(1,2,2);       
%         plot(pts(1,:),pts(2,:),'.k');
%          hold on, 
%         subplot(1,2,2);
%         plot(c1,r1,'r+');  % r1 <-> x, c1 <-> y
        % end math plot
        
        % PLOT with image coord
        axis('equal')
        subplot(1,2,2);       
        hold on;
        plot(pts0(1,:),pts0(2,:),'.g');
        hold on, 
        subplot(1,2,2);
        plot(c1_0,r1_0,'r+');  % r1 <-> x, c1 <-> y
        % end image coord plot
%         try
%             plotmodel(fig,modeli,'image',size(BW,1));
%             
%         catch err
%             error('first image does not have "modeli"!!');
%         end
        
        % output: pts point data of the image
        points = pts;
        r = r1;
        c = c1;
        imgH = size(BW,1);
        imgW = size(BW,2);
    end

end